#include "foc.h"

DutyStruct DutyText[2];
MotorParaStruct motor_para;
arm_pid_instance_f32 pid_para_position;
arm_pid_instance_f32 pid_para_speed;
arm_pid_instance_f32 pid_para_torque_d;
arm_pid_instance_f32 pid_para_torque_q;
float encoder_angle;
float diff_angle;
float mul_cycle_angle;
float rotor_zero_angle;
/************************************** */
//function : Svpwm 
//@brief :  svpwm   clack
//para : @angel  destination angel  unit deg
//para : @s      power  from 0 to 1
//return : DutyStruct
//δ���ԣ����Ǻ�����abs��ϲ������ￄ1�71ￄ1�771ￄ1�71ￄ1�777
/************************************** */
DutyStruct Svpwm(int theta,float s)
{
    DutyStruct dutytmp;
    int v[6][3] = {{1,0,0},{1,1,0},{0,1,0},{0,1,1},{0,0,1},{1,0,1}};
    int sector = 1 + (theta / rad60);
    float t0,tm,tn;

    tm = arm_sin_f32(sector*rad60 - theta);
    tn = arm_cos_f32(theta - (sector*rad60 - rad60));
    t0 = 1-tm-tn;
    dutytmp.u = tm*s*v[sector-1][0] + tn*s*v[sector%6][0] - t0/2; 
    dutytmp.v = tm*s*v[sector-1][1] + tn*s*v[sector%6][1] - t0/2; 
    dutytmp.w = tm*s*v[sector-1][2] + tn*s*v[sector%6][2] - t0/2; 

    return dutytmp;
}
/************************************** */
//function : DescarSvpwm 
//@brief :  foc to  svpwm   invpark
//para : @phi    phi unit rad from  0  to  aim/7  
//para : @d      power d from 0 to 1
//para : @q      power q from 0 to 1
//return : DutyStruct
/************************************** */
DutyStruct DescarSvpwm(float phi,float d,float q)
{
	  d = MIN(d, 1);
    d = MAX(d, -1);
    q = MIN(q, 1);
    q = MAX(q, -1);
    DutyStruct dutytmp;
    float alaph,beta;
    float t0,tm,tn;
    float sin_phi = arm_sin_f32(phi);
    float cos_phi = arm_cos_f32(phi);
    int K,sector,Ka,Kb,Kc;
    const int v[6][3] = {{1,0,0},{1,1,0},{0,1,0},{0,1,1},{0,0,1},{1,0,1}};
    const int K2sector[7] = {4,6,5,5,3,1,2};

    /*********************inv park **********************/	
    alaph = cos_phi*d - sin_phi*q;
    beta  = sin_phi*d + cos_phi*q;
    //arm_inv_park_f32(d, q, &alaph, &beta, sin_phi, cos_phi);

    /*********************sector select **********************/	
    Ka = beta > 0;
    Kb = fabsf(beta) > (SQRT3 * fabsf(alaph));
    Kc = alaph > 0;
	
//	   Ka = beta > 0;
//     Kb = fabs(beta) > SQRT3 * fabs(alaph);
//     Kc = alaph > 0;	

		K = 4*Ka + 2*Kb + Kc;
		sector = K2sector[K];
		
    /*********************u v  w  calculate **********************/	
    tm = arm_sin_f32(sector*rad60)*alaph -arm_cos_f32(sector*rad60)*beta;
    tn = arm_cos_f32(sector*rad60 - rad60)*beta - arm_sin_f32(sector*rad60 - rad60)*alaph;
    t0 = 1-tm-tn;
    dutytmp.u = tm*v[sector-1][0] + tn*v[sector%6][0] + t0/2; 
    dutytmp.v = tm*v[sector-1][1] + tn*v[sector%6][1] + t0/2; 
    dutytmp.w = tm*v[sector-1][2] + tn*v[sector%6][2] + t0/2; 
	return dutytmp;
}

/************************************** */
//function : MotorForword 
//@brief :  foc motor control
//para : @phi    phi unit rad from  0  to  aim/7  
//para : @d      power d from 0 to 1
//para : @q      power q from 0 to 1
//return : DutyStruct
/************************************** */
void MotorForword(float d,float q,float phi)
{
	DutyStruct dutytmp;
	dutytmp = DescarSvpwm(phi,d,q);
	DutyText[1] = dutytmp;
	set_pwm_duty(dutytmp.u,dutytmp.v,dutytmp.w);
}

void set_pwm_duty(float d_u, float d_v, float d_w)
{
  d_u = MIN(d_u, 0.9);
  d_v = MIN(d_v, 0.9);
  d_w = MIN(d_w, 0.9);
  __disable_irq();
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, d_u * htim1.Instance->ARR);
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, d_v * htim1.Instance->ARR);
  __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, d_w * htim1.Instance->ARR);

  __enable_irq();
}
void foc_forward(float d, float q, float rotor_rad)
{
    float d_u = 0;
    float d_v = 0;
    float d_w = 0;
    svpwm(rotor_rad, d, q, &d_u, &d_v, &d_w);
	DutyText[0].u = d_u;
    DutyText[0].v = d_v;
    DutyText[0].w = d_w;
    set_pwm_duty(d_u, d_v, d_w);
}
static void svpwm(float phi, float d, float q, float *d_u, float *d_v, float *d_w)
{
    d = MIN(d, 1);
    d = MAX(d, -1);
    q = MIN(q, 1);
    q = MAX(q, -1);
    const int v[6][3] = {{1, 0, 0}, {1, 1, 0}, {0, 1, 0}, {0, 1, 1}, {0, 0, 1}, {1, 0, 1}};
    const int K_to_sector[] = {4, 6, 5, 5, 3, 1, 2, 2};
    float sin_phi = arm_sin_f32(phi);
    float cos_phi = arm_cos_f32(phi);
    float alpha = 0;
    float beta = 0;
    arm_inv_park_f32(d, q, &alpha, &beta, sin_phi, cos_phi);

    bool A = beta > 0;
    bool B = fabs(beta) > SQRT3 * fabs(alpha);
    bool C = alpha > 0;

    int K = 4 * A + 2 * B + C;
    int sector = K_to_sector[K];

    float t_m = arm_sin_f32(sector * rad60) * alpha - arm_cos_f32(sector * rad60) * beta;
    float t_n = beta * arm_cos_f32(sector * rad60 - rad60) - alpha * arm_sin_f32(sector * rad60 - rad60);
    float t_0 = 1 - t_m - t_n;

    *d_u = t_m * v[sector - 1][0] + t_n * v[(sector) % 6][0] + t_0 / 2;
    *d_v = t_m * v[sector - 1][1] + t_n * v[(sector) % 6][1] + t_0 / 2;
    *d_w = t_m * v[sector - 1][2] + t_n * v[(sector) % 6][2] + t_0 / 2;
}

/************************************** */
//function : Filter 
//@brief :  Filtper 
//para : @in    
//para : @filter_type      
//return : DutyStruct
/************************************** */
float LowpassFilter(float in,float last_out,float alaph)
{
    return (1 - alaph)*last_out + alaph*in;
}
/************************************** */
//function : Filter 
//@brief :  Filtper 
//para : @in    
//para : @filter_type      
//return : DutyStruct
/************************************** */
float HalfCycle(float angel,float cycle)
{
    if(angel > (cycle/2))
    {
      angel -= cycle;
    }
    else if(angel < (-cycle / 2))
    {
      angel += cycle;
    }
    return angel;
    
}
/************************************** *************/
/* arm_pid_f32() pid 公式推导
   *    A0 = Kp + Ki + Kd
   *    A1 = (-Kp ) - (2 * Kd )
   *    A2 = Kd
   *    y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
   *         = y[n-1] + Kp( x[n] -x[n-1])  + Ki( x[n])  +Kd( x[n] -x[n-1] +x[n-2] - x[n-1])    
   *                     
   *   y[n] =y[n-1]+          A0 * x[n] +A1 * x[n-1] + A2 * x[n-2]  
   *   out = (S->state[2]) + (S->A0 * in) +(S->A1 * S->state[0]) + (S->A2 * S->state[1]) + ;
   * 
		Update state 

   *  S->state[1] = S->state[0]; 
   *  S->state[0] = in;
   *  S->state[2] = out;
 */ 

#if UNUSE_ARM_MATH_LIB  //arm_math 库中函数 未制作完戄1�7
__STATIC_FORCEINLINE float32_t arm_pid_f32(
  arm_pid_instance_f32 * S,
  float32_t in)
  {
    float32_t out;

    /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]  */
    out = (S->A0 * in) +
      (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);

    /* Update state */
    S->state[1] = S->state[0];
    S->state[0] = in;
    S->state[2] = out;

    /* return to application */
    return (out);
  }
/************************************** *************/

/************************* sin ********** ***********/
    /**
   * @brief  Fast approximation to the trigonometric sine function for floating-point data.
   * @param[in] x  input value in radians.
   * @return  sin(x).
   */
  float32_t arm_sin_f32(
  float32_t x);
/************************************** *************/

/************************* cos ********** ***********/
  /**
   * @brief  Fast approximation to the trigonometric cosine function for floating-point data.
   * @param[in] x  input value in radians.
   * @return  cos(x).
   */
  float32_t arm_cos_f32(
  float32_t x);

/************************************** *************/
#endif
/************************************* **************/
uint8_t CalculateCrc(uint32_t data)
{
  uint8_t crc = 0;
  uint32_t polynomial = 0x43; // (X^6 + X + 1)鐨勪簩杩涘埗鍊�

  // (D[13:0] 鍜� Mg[3:0])
  for (int i = 17; i >= 0; i--)
  {
    uint8_t bit = (data >> i) & 1;
    crc <<= 1;
    if ((crc >> 6) ^ bit)
      crc ^= polynomial;
    crc &= 0x3F;
  }
  return crc;
}
void PidPositionControl(float rad)
{
  static float d,q;
  d = 0;
  q = PositionLoop(rad);
  MotorForword(d,q,ROTOR_LOGIC_ANGLE);
  //foc_forward(d,q,ROTOR_LOGIC_ANGLE);
}
void PidParaInit(void)
{
  pid_para_position.Kp = POSITION_KP;
  pid_para_position.Ki = POSITION_KI;
  pid_para_position.Kd = POSITION_KD;

  pid_para_speed.Kp = SPEED_KP;
  pid_para_speed.Ki = SPEED_KI;
  pid_para_speed.Kd = SPEED_KD;

  pid_para_torque_d.Kp = TORQUE_D_KP;
  pid_para_torque_d.Ki = TORQUE_D_KI;
  pid_para_torque_d.Kd = TORQUE_D_KD;

  pid_para_torque_q.Kp = TORQUE_Q_KP;
  pid_para_torque_q.Ki = TORQUE_Q_KI;
  pid_para_torque_q.Kd = TORQUE_Q_KD;
}
float PositionLoop(float rad)
{
    float diff;
    diff = HalfCycle(rad - mul_cycle_angle,MAX_MUL_CYCLE);
    //return arm_pid_f32(&pid_para_position,diff);
		return diff;
}
void PidSpeedControl(float speed){}
void PidTorqueControl(float d,float q){}
void PidSpeedTorqueControl(float speed,float d,float q){}
void PidPositionSpeedCtorqueControl(float rad,float speed,float d,float q){}
